//
// Created by jg on 2021/1/4.
//

#ifndef WMR_PLAN_MANAGE_PLANNER_MANAGER_H
#define WMR_PLAN_MANAGE_PLANNER_MANAGER_H

#include <iostream>
#include <iomanip>
#include <fstream>
#include <vector>
#include <queue>
#include <list>
#include <mutex>

#include <ros/ros.h>
#include <memory>
#include <nav_msgs/Path.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/PointCloud2.h>

#include <env_edt/plan_map.h>
#include <fast_marching/fast_marching.h>
#include <bezier/bezier_base.h>
#include <bezier_opt/trajectory_generator.h>
#include <pos_generator/oriTrans.hpp>
#include <pos_generator/gazebo_to_pos.h>
#include <pos_generator/insprobe_to_pos.h>
#include <wmr_nav/VSTTM_nav.h>
#include <vis_rviz/vis_rviz.h>
#include <topo_prm/topo_prm.h>
#include <bspline/non_uniform_bspline.h>
#include <bspline_opt/bspline_optimizer.h>
#include <active_location_path/active_location_path.h>

#include <insprobe_msgs/Navigation.h>
#include "wmr_plan_manage/plan_container.hpp"

using std::cout;
using std::endl;
using std::list;
using std::string;
using std::vector;
using std::queue;
using std::shared_ptr;
using std::unique_ptr;

class PlanManager{
protected:
    /* 由于点云信息相比定位晚0.1s，故使用容器容纳定位数据，
     * 将延时的点云与对应时间的定位匹配，但规划仍用最新数据 */
    struct ROBOTSTATE{
        double time, yaw;
        Eigen::Vector3d pt;

        ROBOTSTATE(double _time, Eigen::Vector3d& _pt, double _yaw){
            time = _time;
            yaw  = _yaw;
            pt   = _pt;
        }
        ROBOTSTATE(): time(-1), yaw(-1), pt(Eigen::Vector3d::Ones()){}
    };

    /* ---------- flag ---------- */
    enum PLAN_EXEC_STATE {
        INIT, WAIT_TARGET, GEN_NEW_TRAJ, REPLAN_TRAJ, EXEC_TRAJ, REPLAN_NEW, ANGLE_PID_CTRL
    };
    enum TARGET_TYPE {
        MANUAL = 1, AUTO, PRESET_TARGET, REFENCE_PATH
    };
    enum PLATFORM {
        SIMULATION = 0, REAL
    };

    /* planning data */
    bool has_pos_, has_target_, use_ground_, collide_;
    bool is_angle_PID_, PID_finish_, is_active_location_path_;
    PLAN_EXEC_STATE exec_state_;

    MAP_EDT::Ptr   map_edt_;
    Path_FMM::Ptr  path_FMM_;
    Bernstein::Ptr bezier_base_;
    VSTTM::Ptr     nav_VSTTM_;
    VIS_RVIZ::Ptr  vis_rviz_;

    AL_Path al_path_;

    GAZEBO_POS   gazebo_pos_;
    INSPROBE_POS insprobe_pos_;
    TrajectoryGenerator bezier_opt_;

    ros::Publisher  cmd_pub_, path_pub_;
    ros::Subscriber wp_rviz_sub_, wp_real_sub, pos_sub_, obj_sub_, grp_sub_;
    ros::Timer      exec_timer_, safety_timer_, gazebo_timer_, vis_global_map_;

    Eigen::Vector3d start_pos_, start_vel_, start_acc_, end_pos_, end_vel_, end_acc_, last_vel_;
    double          robot_yaw_, robot_vel_, robot_avl_;
    ros::Time       time_now_,  time_last_;

    double          replan_distance_threshold_, replan_time_threshold_;

    PlanParameters     pp_;
    BezierParameters   bp_;
    GlobalTrajData     global_data_;
    LocalTrajData      local_data_;
    nav_msgs::Odometry rob_state_;
    MidPlanData        plan_data_;

    unique_ptr<TopologyPRM> topo_prm_;
    vector<BsplineOptimizer::Ptr> bspline_opt_;

    queue<Eigen::Vector3d>  target_;
    vector<Eigen::Vector3d> raw_target_;
    queue<MyPcd::Ptr> ground_pcd_, obs_pcd_;
    list<ROBOTSTATE>  list_state_;

    std::string path_name_;
    std::ofstream out_file_;

    int platform_;
    std::mutex pos_lock_, file_lock_;
public:
    PlanManager() : has_pos_(false), has_target_(false),
    use_ground_(false), collide_(false),
    is_angle_PID_(false), PID_finish_(false), is_active_location_path_(false),
    exec_state_(INIT), robot_yaw_(0.0),
    replan_distance_threshold_(-1), replan_time_threshold_(-1),
    pp_(-1, -1, -1), platform_(-1){
        robot_yaw_ = 0.0;
        robot_vel_ = 0.0;
        robot_avl_ = 0.0;

        last_vel_  = Eigen::Vector3d::Zero();
        time_last_ = ros::Time::now();

        bp_.traj_order = -1;
        bp_.minimize_order = -1.0;

        local_data_.is_traj_start_ = false;
        local_data_.traj_id_ = 0;
    };
    ~PlanManager(){
        out_file_.close();
    };

    void init(ros::NodeHandle& nh);

protected:
    void rcvINSPROBECallback(const insprobe_msgs::Navigation &msg);
    void rcvGazeboPcdCallback(const sensor_msgs::LaserScan &msg);
    void rcvObjPcdCallback(const sensor_msgs::PointCloud2 &msg);
    void rcvGroundPcdCallback(const sensor_msgs::PointCloud2 &msg);

    void rcvGazeboGoal(const geometry_msgs::PoseStamped::ConstPtr& msg);
    void rcvRealGoal(const geometry_msgs::Pose2D& msg);
    void rcvWaypointCallback(const nav_msgs::Path &wp);

    void positionHandle(const nav_msgs::Odometry &state);

    void changeFSMExecState(PLAN_EXEC_STATE new_state, string pos_call);
    void printPLANExecState();

    void execPlanCallback(const ros::TimerEvent& e);
    void generateGazeboPos(const ros::TimerEvent& e);
    void checkCollisionCallback(const ros::TimerEvent& e);
    void visGlobalMap(const ros::TimerEvent& e);
    bool checkTrajCollision(double& distance);

    bool FMMPathSearching();
    bool bezierTrajGenerating();
    bool topoReplan();
    void findCollisionRange(vector<Eigen::Vector3d>& colli_start, vector<Eigen::Vector3d>& colli_end,
                            vector<Eigen::Vector3d>& start_pts, vector<Eigen::Vector3d>& end_pts);
    void optimizeTopoBspline(double start_t, double duration, vector<Eigen::Vector3d>& guide_path, int traj_id);

    Eigen::MatrixXd reparamLocalTraj(double start_t, double duration, int seg_num, double& dt);
    Eigen::MatrixXd reparamLocalTraj(double start_t, double& dt, double& duration);

    void refreshMap();
    void stopRobot(bool is_keep_on_nav = false);

    void selectBestTraj(NonUniformBspline& traj);
    void refineTraj(NonUniformBspline& best_traj, double &time_inc);
    void reparamBspline(NonUniformBspline& bspline, double ratio, Eigen::MatrixXd& ctrl_pts, double& dt,
                        double& time_inc);

    bool planGlobalTraj();

    void updataLocalTraj();

    bool isReachGlobalTarget();
    bool isReplan();
    bool anglePIDCtrl();

    void activeLocationPath();
    void pubBLHPath(vector<Eigen::Vector3d>& _path);

    void visBezierTraj();

    void getSubTarget();
};

#endif //WMR_PLAN_MANAGE_PLANNER_MANAGER_H
